/*
brush.c
offer brush motor driver controller function
designed by lunanting

----------------------
2020-03-25 17:34:44
create file
----------------------
*/
#include "brush.h"
#include "../../include/aos/kernel.h"

static BRUSH_state brush_state={0};
static float g_current_r[BRUSH_MAX]= {0};

/*
brush motor config run state
param:
    run:run type
*/
void xp_brush_run_config(BRUSH_run run){

    switch (run)
    {
    case brush_run_no:
        brush_state.run_data&=0xffffc;
        break;
    case brush_run_start:
        brush_state.run_data|=0x0002;
        brush_state.run_data&=0xfffe;
        break;
    case brush_run_stop:
        brush_state.run_data|=0x0001;
        brush_state.run_data&=0xfffd;
        break;
    case brush_run_jog:
        brush_state.run_data|=0x0003;
        break;
    default:
        brush_state.run_data&=0xffffc;
        break;
    }
}

/*
brush motor config dir
param:
    dir:dir type
*/
void xp_brush_dir_config(BRUSH_dir dir){
    switch (dir)
    {
    case brush_dir_no:
        brush_state.run_data&=0xffcf;
        break;
    case brush_dir_a:
        brush_state.run_data|=0x0010;
        brush_state.run_data&=0xffdf;
        break;
    case brush_dir_b:
        brush_state.run_data|=0x0020;
        brush_state.run_data&=0xffef;    
        break;
    case brush_dir_change:
        brush_state.run_data|=0x0030;
        break;
    default:
        brush_state.run_data&=0xffcf;
        break;
    }
}


/*
brush motor expedit /moderate config
param:
    exp:exp/mod choose
*/
void xp_brush_exp_config(BRUSH_exp exp){
    switch (exp)
    {
    case brush_exp_1:
        brush_state.run_data&=0xff3f;
        break;
    case brush_exp_2:
        brush_state.run_data&=0xff7f;
        brush_state.run_data|=0x0040;
        break;
    case brush_exp_3:
        brush_state.run_data&=0xffBf;
        brush_state.run_data|=0x0080;
        break;
    case brush_exp_4:
        brush_state.run_data|=0x00c0;
        break;
    default:
        brush_state.run_data&=0xff3f;
        break;
    }
}


/*
brush motor speed choose
param:
    speed:choose speed
*/
void xp_brush_speed_config(BRUSH_speed speed){
    switch (speed)
    {
    case brush_speed_master:
        brush_state.run_data&=0xf0ff;
        break;
    case brush_speed_1:
        brush_state.run_data|=0x0100;
        brush_state.run_data&=0xf1ff;
        break;
    case brush_speed_2:
        brush_state.run_data|=0x0200;
        brush_state.run_data&=0xf2ff;
        break;
    case brush_speed_3:
        brush_state.run_data|=0x0300;
        brush_state.run_data&=0xf3ff;
        break;
    default:
        brush_state.run_data&=0xf0ff;
        break;
    }
}

/*
brush motor enable exp and speed
param:
    state:0=disable,1=enable
*/
void xp_brush_choose_enable(uint8_t state){
    if(state==0){
        brush_state.run_data&=0xefff;
    }
    else{
        brush_state.run_data|=0x1000;
    }
}



/*
brush motor ask
return:
    0=success

*/
int xp_brush_ask(void){
    int ret=0;
    MB_data data;
    u8 count=5;

    memcpy(&data,&brush_state.modbus,sizeof(MB_data));

    if(brush_state.init!=1){
        println("error:xp_brush_ask() brush motor no init!");
        return -10;
    }
    while(count--){
        ret=xp_modbus_ask(brush_state.modbus_port,&data);
        if(brush_state.read_len<2){
            continue;
        }
        if(ret==0)break;
        aos_msleep(10);
    }
    return ret;
}

/*
brush motor run with choose
param:
    run: run type
    dir:run direction
    exp:expedite or moderate choose
    speed:speed choose
return:
    0=success

*/
int xp_brush_run_choose(BRUSH_run run,BRUSH_dir dir,BRUSH_exp exp,BRUSH_speed speed){
    int ret=0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    xp_brush_run_config(run);
    xp_brush_dir_config(dir);
    xp_brush_exp_config(exp);
    xp_brush_speed_config(speed);
    xp_brush_choose_enable(1);
    brush_state.modbus.start_addr=BRUSH_RUN;
    brush_state.modbus.wBuf=&brush_state.run_data;
    brush_state.modbus.rwLen=1;
    brush_state.modbus.mb_fun=BRUSH_WRITE;

    ret=xp_brush_ask();
    aos_mutex_unlock(&brush_state.mux);
    return ret;
}

/*
brush motor run not with choose
param:
    run:run type
    dir:direction
return:
    0=success

*/
int xp_brush_run(BRUSH_run run,BRUSH_dir dir){
    int ret=0;

    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    xp_brush_run_config(run);
    xp_brush_dir_config(dir);
    xp_brush_choose_enable(0);
    brush_state.modbus.start_addr=BRUSH_RUN;
    brush_state.modbus.wBuf=&brush_state.run_data;
    brush_state.modbus.rwLen=1;
    brush_state.modbus.mb_fun=BRUSH_WRITE;

    ret=xp_brush_ask();
    aos_mutex_unlock(&brush_state.mux);
    return ret;
}

/*
brush motor run use package
param:
    conf:run config package
return:
    0=success

*/
int xp_brush_run_pack(BRUSH_conf *conf){
    int ret=0;

    if(conf->choose_en==0){
        ret=xp_brush_run(conf->run,conf->dir);
    }
    else{
        ret=xp_brush_run_choose(conf->run,conf->dir,conf->exp,conf->speed);
    }
    return ret;
}


/***************************set motor message or run*******************/
/*
brush motor set hz
param:
    hz:set hz value
return:
    0=success

*/
int xp_brush_set_hz(float hz){
    int ret=0;
    //add get hz data .....
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.hz_data=(int)(hz*100);
    brush_state.modbus.start_addr=BRUSH_HZ;
    brush_state.modbus.wBuf=&brush_state.hz_data;
    brush_state.modbus.rwLen=1;
    brush_state.modbus.mb_fun=BRUSH_WRITE;

    ret=xp_brush_ask();
    aos_mutex_unlock(&brush_state.mux);
    return ret;
}



/*
brush motor reset
return:
    0=success
*/
int xp_brush_reset(void){
    int ret=0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.reset_data=0x0002;
    brush_state.modbus.start_addr=BRUSH_RESET;
    brush_state.modbus.wBuf=&brush_state.reset_data;
    brush_state.modbus.rwLen=1;
    brush_state.modbus.mb_fun=BRUSH_WRITE;

    ret=xp_brush_ask();
    aos_mutex_unlock(&brush_state.mux);
    return ret;
}


/*
set bursh motor stop
return:
    0=set success
*/
int xp_brush_stop(void){
	brush_state.run_timestamp=0;
    return xp_brush_run(brush_run_stop,brush_dir_no);
}

/*
set brush motor  正转
return:
    0=set success
*/
int xp_brush_foreward(void){
	brush_state.run_timestamp=aos_now_ms();
    return xp_brush_run(brush_run_start,brush_dir_a);
}

/*
set brush motor 反转
return:
    0=set success
*/
int xp_brush_reversal(void){
	brush_state.run_timestamp=aos_now_ms();
    return xp_brush_run(brush_run_start,brush_dir_b);
}

/*
brush set cmd
param:
    cmd: 0=stop,1=forwad,-1=reversal
return:
    0=success
*/
int xp_brush_cmd_set(int cmd){
    if(cmd==0){
        xp_brush_stop();
    }
    else if(cmd<0){
        xp_brush_reversal();
    }
    else if(cmd>0){
        xp_brush_foreward();
    }
}

/****************************get motor message*******************/
/*
get brush motor error/warn code
return:
    code: code value
*/
uint16_t xp_brush_get_code(void){
    int ret=0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.modbus.mb_fun=BRUSH_READ;
    brush_state.modbus.start_addr=BRUSH_CODE;
    brush_state.modbus.rwLen=1;

    ret=xp_brush_ask();
    aos_mutex_unlock(&brush_state.mux);
    if(ret!=0)return 0;
    return (brush_state.read_state[1]<<8)+brush_state.read_state[0];
}


/*
get brush motor run state
return:
    state:state value
*/
uint16_t xp_brush_get_state(void){
    int ret=0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.modbus.mb_fun=BRUSH_READ;
    brush_state.modbus.start_addr=BRUSH_STATE;
    brush_state.modbus.rwLen=1;

    ret=xp_brush_ask();
    aos_mutex_unlock(&brush_state.mux);
    if(ret!=0)return 0;
    return brush_state.read_state[0]+(brush_state.read_state[1]<<8);
}



/*
get brush motor current
return:
    current value
*/
float xp_brush_get_current(void){
    int ret=0;
    static float recode=0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.modbus.mb_fun=BRUSH_READ;
    brush_state.modbus.start_addr=BRUSH_CURRENT;
    brush_state.modbus.rwLen=1;

    ret=xp_brush_ask();
    int c=brush_state.read_state[0]+(brush_state.read_state[1]<<8);
    aos_mutex_unlock(&brush_state.mux);
    if(ret!=0)return recode;
     recode=c/100.0;
    //int c=brush_state.read_state[1]+(brush_state.read_state[0]<<8);
    //recode=c;
    return recode;
}

/*
get brush torque percent
return:
    current value
*/

float xp_brush_get_torque(void)
{
    int ret = 0;
    float recode = 0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.modbus.mb_fun=BRUSH_READ;
    brush_state.modbus.start_addr=BRUSH_TORQUE;
    brush_state.modbus.rwLen=1;
    if(brush_state.init==0)return 0;
    ret=xp_brush_ask();
    int c=brush_state.read_state[1]+(brush_state.read_state[0]<<8);
    aos_mutex_unlock(&brush_state.mux);
    if(ret!=0)return recode;
    recode=c;
    return recode;

}

/*
get brush actual speed 
return:
    current value
*/
int xp_brush_get_speed(void)
{
    int ret = 0;
    if(aos_mutex_lock(&brush_state.mux,500)!=0){
        return -1;
    }
    brush_state.modbus.mb_fun=BRUSH_READ;
    brush_state.modbus.start_addr=BRUSH_SPEED;
    brush_state.modbus.rwLen=1;

    ret=xp_brush_ask();
    int c=brush_state.read_state[1]+(brush_state.read_state[0]<<8);
    aos_mutex_unlock(&brush_state.mux);
    if(ret!=0)return 0;
    return c;
}



/*
brush motor init
param:
    port:485 port
    slave_id:brush modbus id
*/
int xp_brush_init(uint8_t port,uint8_t slave_id){
    brush_state.modbus_port=port;
    brush_state.modbus.slave_id=slave_id;
    brush_state.init=0;
    if(xp_modbus_init(port,115200,MB_PAR_NONE,100)!=0){
        println(" brush modbus init fail~");
        return -1;
    }
    brush_state.init=1;
    brush_state.run_data=0;
    brush_state.hz_data=0;
    memset(brush_state.read_state,0,10);
    brush_state.reset_data=0x0002;
    brush_state.modbus.resBuf=brush_state.read_state;
    brush_state.modbus.resLen=&brush_state.read_len;
    brush_state.read_current=0;
    aos_mutex_new(&brush_state.mux);
    void xp_brush_read_current_thread(void *arg);
    aos_task_new("read brush current",xp_brush_read_current_thread,NULL,1024);
    if(xp_brush_reset()!=0)return -3;
    if(xp_brush_stop()!=0){
        brush_state.init=0;
        println("brush init stop fail~");
        return -2;
    }
    println("brush motor init success~");
    return 0;
}

int xp_brush_recover(uint8_t port,uint8_t slave_id){
    brush_state.modbus_port=port;
    brush_state.modbus.slave_id=slave_id;
    if(xp_modbus_init(port,115200,MB_PAR_NONE,100)!=0){
        println(" brush modbus recover fail~");
        return -1;
    }
    brush_state.run_data=0;
    brush_state.hz_data=0;
    memset(brush_state.read_state,0,10);
    brush_state.reset_data=0x0002;
    brush_state.modbus.resBuf=brush_state.read_state;
    brush_state.modbus.resLen=&brush_state.read_len;
    brush_state.read_current=0;
    brush_state.press_stop = 0;

    if(xp_brush_reset()!=0)return -3;
    if(xp_brush_stop()!=0){
        println("brush recover stop fail~");
        return -2;
    }
    println("brush motor recover success~");
    return 0;
}

/*
get motor current thread
*/
void xp_brush_read_current_thread(void *arg){
    float torque =0 ;
    int16_t speed = 0;
	int press_count=0;
	int brush_not_ack_cnt = 0;
	uint16_t count = 0;
    while(true){
        if(brush_state.read_current==1){
            brush_state.current=xp_brush_get_torque();
           // xp_brush_current_record(brush_state.current);  
            if(brush_state.current>10000)
            {
                brush_state.current = 65536- brush_state.current;
            }
			xp_brush_current_record(brush_state.current);

			if (brush_state.current > 1200){
				if (press_count++ > 3){
					brush_state.press_stop = 1;
					println("!!!Brush STOP (over press)\r\n");
				}
			}else{
				press_count = 0;
			}
			
			if (0 != brush_state.run_timestamp && aos_now_ms() - brush_state.run_timestamp > 5000){
				if (brush_state.current < 50){
					if (brush_not_ack_cnt++ > 3){
						brush_state.press_stop = 1;
						println("!!!Brush STOP (not press)\r\n");
					}
				}else{
					brush_not_ack_cnt = 0;
				}
			}
			
			if (count++ % 5 == 0 || press_count){
				//print("current:%d\r\n", (int)brush_state.current);
			}
            //println("brush current:%0.3f A",brush_state.current);
            //torque = xp_brush_get_torque();
            //print(" current:%0.3f torque %f speed %d  \n\r",brush_state.current,torque,speed);
        }
        aos_msleep(50);
    }
}


/*
read brush current
return:current value
*/
float xp_brush_read_current(void){
    return brush_state.current;
}

/*
start read current
param:
    sta:0=stop,1=start
*/
void xp_brush_start_read_current(u8 sta){
    if(sta==0){
        brush_state.read_current=0;
    }
    else{
        brush_state.read_current=1;
    }
}
/*
滑动窗口记录刷子电流的值,下标为0 的时候是最新的值
*/
void xp_brush_current_record(float cunrent_temf)
{
    u16 i =BRUSH_MAX-1;
    
    for(;i>0;i--)
    {
        g_current_r[i]=g_current_r[i-1];
    }
    g_current_r[0]= cunrent_temf;
    return;
}
/*
    求滑动窗口记录的前n个下标均值
*/

float xp_brush_average_current(u16 index)
{
    u16 i = 0;
    float sum_current = 0;
    if(index < 0 || index >BRUSH_MAX)
    {
        index = BRUSH_MAX;
    }
    for(;i<index;i++)
    {
       sum_current = sum_current+g_current_r[i];
    }
    return (sum_current/index);

}


/*
get brush press error
return:state
	0=normal
	1=touched
*/
int xp_brush_press_stop_get(void){
	return brush_state.press_stop;
}



/**********************DEBUG***************************/
int xp_brush_debug(char *type,char *fun,char *param){
    float average_c = 0;
    u16 index = 0;
    if(strcmp(type,"brush")!=0)return 0;
    if(strcmp(fun,"fw")==0){
        xp_brush_foreward();        
    }
    else if(strcmp(fun,"back")==0){
        xp_brush_reversal();
    }
    else if(strcmp(fun,"stop")==0){
        xp_brush_stop();   
    }
    else if(strcmp(fun,"read_current")==0){
        xp_brush_start_read_current(atoi(param));
    }
    else if(strcmp(fun,"read_averagec")==0)
    { 
        //index = atoi(param);
        //println("******we will to caculate the average current  %d *****",index);
        //average_c = xp_brush_average_current(index);
        println("xp_brush_average_current %f", xp_brush_read_current());
    }
    else if(strcmp(fun,"read_record")==0)
    {
        printf("\n******************\n");
        for(;index<BRUSH_MAX;index++)
        {
          println("index %d record current is  %f ",index,g_current_r[index]);  
        }
        printf("\n******************\n");

    }
    else if(strcmp(fun,"speed")==0){
        int s=atoi(param);
        if(s<4){
            xp_brush_speed_config(s);
        }
        else
        {
            println("brush speed set fail~");
        }
    }
    else if(strcmp(fun,"set_freq")==0){
        xp_brush_set_hz(atoi(param));
    }
    else if(strcmp(fun,"reset")==0){
        xp_brush_reset();
    }
    else{
        return 0;
    }
    return 1;
}


//end of the file
